
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mp_steer_controller.h
  * @author     baiyang
  * @date       2023-7-5
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <pid/pid.h>
#include <pid/pid_info.h>

#include <common/gp_defines.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
typedef struct mp_steer_controller *mp_steer_controller_t;

struct mp_steer_controller {
    Param_float _tau;
    Param_float _K_FF;
    Param_float _K_P;
    Param_float _K_I;
    Param_float _K_D;
    Param_float _minspeed;
    Param_int16  _imax;
    uint32_t _last_t;
    float _last_out;

    Param_float _deratespeed;
    Param_float _deratefactor;
    Param_float _mindegree;

    struct pid_info _pid_info;

    bool _reverse;
};
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sctrl_ctor(mp_steer_controller_t steer_ctrl);

// steering rate controller. Returns servo out -4500 to 4500 given
// desired yaw rate in degrees/sec. Positive yaw rate means clockwise yaw.
int32_t sctrl_get_steering_out_rate(mp_steer_controller_t steer_ctrl, float desired_rate);

// lateral acceleration controller. Returns servo value -4500 to 4500
// given a desired lateral acceleration
int32_t sctrl_get_steering_out_lat_accel(mp_steer_controller_t steer_ctrl, float desired_accel);

// return a steering servo value from -4500 to 4500 given an angular
// steering error in centidegrees.
int32_t sctrl_get_steering_out_angle_error(mp_steer_controller_t steer_ctrl, int32_t angle_err);

// Returns true if controller has been run recently
bool sctrl_active(mp_steer_controller_t steer_ctrl);

static inline void sctrl_reset_i(mp_steer_controller_t steer_ctrl) { steer_ctrl->_pid_info.I = 0; }

// return the steering radius (half diameter). Assumed to be half
// the P value.
static inline float sctrl_get_turn_radius(mp_steer_controller_t steer_ctrl) { return steer_ctrl->_K_P * 0.5f; }

static inline pid_info_const_t sctrl_get_pid_info(mp_steer_controller_t steer_ctrl) { return &steer_ctrl->_pid_info; }
static inline void sctrl_set_reverse(mp_steer_controller_t steer_ctrl, bool reverse) { steer_ctrl->_reverse = reverse; }
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



